/*********************************************************************************

    @file      Engine_OPS.c                                                              
    @date      December, 2011
    @brief     Open5xxxECU - this file contains functions for fuel pulse width 
               as well as spark timing 
    @note      www.Open5xxxECU.org
    @version   2.1
    @copyright MIT License

**********************************************************************************/

/*================================================================================
   ORIGINAL AUTHOR:  Paul Schlein                                                 
   REV      AUTHOR          DATE          DESCRIPTION OF CHANGE                   
   ---     -----------     ----------    ---------------------                    
   2.1     J. Zeeff        28/Dec/11     Cleanup                                  
   2.0     M. Eberhardt    22/Dec/11     Rewrote to better use OS features        
   1.0     P. Schlein      12/Sep/11     Initial version with Placeholders        
================================================================================*/

/*
Copyright (c) 2011 

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

*/

#include "config.h"
#include "system.h"

#ifndef MPC5602

#include "main.h"
#include "variables.h"
#include "etpu_toothgen.h"
#include "eQADC_OPS.h"
#include "Engine_OPS.h"
#include "Enrichment_OPS.h"
#include "INPUT_OPS.h"
#include "Variable_OPS.h"
#include "OS.h"
#include "Table_Lookup_JZ.h"
#include "etpu_util.h"
#include "etpu_spark.h"
#include "etpu_fuel.h"
#include "etpu_pwm.h"
#include "etpu_app_eng_pos.h"
#include "etpu_crank_auto.h"
#include "eTPU_OPS.h"

// ??
uint32_t *fs_free_param;


/**********************************************************************************/
/*          Done while power on                                                   */
/**********************************************************************************/

// Stuff that don't change very fast ~100 msec

void Slow_Vars_Task(void)
{
    task_open();                // standard OS entry - NOTE: no non-static local variables!
    for (;;) {
        Get_Slow_Op_Vars();
        task_wait(103);         // allow others tasks to run
    }
    task_close();
}                               /* end of slow_vars_task() */

//Blink based on engine position status
void Eng_Pos_Status_BLINK_Task(void)
{
static uint32_t Blink_Rate;
    task_open();            // standard OS entry - NOTE: no non-static local variables! 
       for (;;) {
//Get_Fast_Op_Vars(); // get inputs
       if (fs_etpu_eng_pos_get_engine_position_status() == FS_ETPU_ENG_POS_FULL_SYNC) { 
           Blink_Rate = 998;
           Set_Pin(RPM_SIGNAL_PIN,0);     //Light On  Constant   
           task_wait(Blink_Rate);           // allow others tasks to run          
           Set_Pin(RPM_SIGNAL_PIN ,1);   // Light Off
           
       } else {
           Blink_Rate = 133;
           Set_Pin(RPM_SIGNAL_PIN,0);   // Light ON
           task_wait(Blink_Rate);           // allow others tasks to run          
           Set_Pin(RPM_SIGNAL_PIN ,1);   // Light Off     
        }
        task_wait(Blink_Rate);     // allow others tasks to run 
    }



    task_close();
}                               /* end of RPM_BLINK_task() */
//this works so it's removed to save lines or now
void Fuel_Pump_Task(void)

{
    task_open();                // standard OS entry - NOTE: no non-static local variables! 

    Set_Pin(FUEL_PUMP_PIN,1);   // Prime fuel system, Fuel Pump ON 
    task_wait(10311);           // allow others tasks to run  

    for (;;) {
        if (RPM == 0) {
            Set_Pin(FUEL_PUMP_PIN,0);     //turn pump off if engine not turning 
        } else {
            Set_Pin(FUEL_PUMP_PIN,1);     //Engine is turning so turn Fuel Pump ON 
        }
        task_wait(500);     // allow others tasks to run 
    }

    task_close();
}                               /* end of fuel_pump_task() */



/**********************************************************************************/
/*  Every 10msec while RPM > 0, engine cranking or running                        */
/**********************************************************************************/


#
void Engine10_Task(void)
{
    static uint32_t Dwell_Corr;
    static uint32_t Dead_Time;
    static uint32_t Dead_Time_Corr;
    static uint32_t Channel;
    static uint24_t Spark_Advance_eTPU;
    static uint32_t Spark_Recalc_Angle_eTPU;
    static uint32_t Min_Dwell;
    static uint32_t Max_Dwell;
    static uint32_t error_code;

    task_open();

    // Note: code here IS speed critical

    for (;;) {
        uint32_t i;

        if (RPM == 0) {
            Get_Fast_Op_Vars(); // get inputs and let coils time out
            Dwell = 0;
            Dead_Time = 0;
            // update eTPU global
            // TODO - something missing here
        } else {
            // calc proper spark values
            Get_Fast_Op_Vars(); // get inputs 

            // TODO  - add load sense method selection and calcs. This only works right with 1 bar MAP
            // Load = Get_Load();
//remoded for testing
            //Load = MAP[1];
//remove table for testing, set to 30 deg BTDC
            //Spark_Advance = 12750; //Table_Lookup_JZ(RPM, Load, Spark_Advance_Table);

            //Calculate angles for eTPU use, must reference the missing tooth, not TCD
            Spark_Advance_eTPU = (Spark_Advance + Engine_Position)<<2;
            if (Spark_Advance_eTPU > 72000)
               Spark_Advance_eTPU -= 72000;
//added for testing            
            Spark_Advance_eTPU = 0; //for testing only
            // TODO Knock_Retard();

            // Dwell - TODO, can this be skipped under some conditions?
//Remove table for testing
            //Dwell_Set = 3000; //for testing need to add a table lookup for this
            Dwell_Corr = 1<<13;//set corr to 1// Table_Lookup_JZ(V_Batt, 0, Dwell_Table);
            Dwell = ( 30000 * Dwell_Corr) >> 13;//Dwell_Set
            Min_Dwell = (Dwell * 102) >> 7;//Dwell_Min
            Max_Dwell = (Dwell * 153) >> 7;//Dwell_Max

            // Calculate an appropriate re-calculation angle for the current Spark_Angle 
            uint32_t Temp1 = (((RPM * Dwell) >> 14) * (uint32_t)(1.2 * (1 << 12)) >>12); // 1.2 is to give the processor time to do the math 
            uint32_t Temp2 = (uint32_t) (.0006 * (1 << 12));     // conversion factor to get Temp1 into deg x 100
            uint32_t Angle_Temp = 72000 - ((Temp1 * Temp2) >> 12);
            Spark_Recalc_Angle_eTPU = (Spark_Advance_eTPU + Angle_Temp);
            if (Spark_Recalc_Angle_eTPU >= 72000)
               Spark_Recalc_Angle_eTPU -= 72000;

            //TODO - Cylinder Trim math and updates
//Pulse_Width = 30000;
            // fuel dead time is done here becasue it depends only on V-Batt which is from Get_Fast_Op_Vars
//Remove table for testing
            Dead_Time_Corr = 1<<13; //Table_Lookup_JZ(V_Batt, 0, Inj_Dead_Time_Table);
            Dead_Time = (1000 * Dead_Time_Corr) >> 13;//Dead_Time_Set
        }

        // Update eTPU with new spark values
        for (i = 0; i < N_Coils; ++i) {
           // This tries until the channel actual updated
             error_code = 1; 
                   while (error_code != 0) 
                   {
                   	error_code = fs_etpu_spark_set_end_angles(Spark_Channels[i], Spark_Advance_eTPU, ((Spark_Advance_eTPU + 36000) % 72000)); 
                   } 
            // This tries until the channel actual updated
             error_code = 1; 
                   while (error_code != 0)                  
                   {                   	                        
                   error_code = fs_etpu_spark_set_end_angles(Spark_Channels[i], Spark_Advance_eTPU, ((Spark_Advance_eTPU + 36000) % 72000)); 
                   }
           // This tries until the channel actual updated
            error_code = 1; 
                   while (error_code != 0)
                   {
                   	error_code = fs_etpu_spark_set_dwell_times(Spark_Channels[i], Dwell, (Dwell * Ignition_Type));
                   }
                     // TODO skip this under most conditions

        }
        // These are global, so only do it once 
        //fs_etpu_spark_set_recalc_offset_angle (Spark_Channels[0], Spark_Recalc_Angle_eTPU);
        fs_etpu_spark_set_min_max_dwell_times (Spark_Channels[0], Min_Dwell, Max_Dwell); 

        // Update Tach signal TODO fix to use Pulses_Per_Rev
       // uint32_t frequency = ((RPM * N_Cyl) * (uint32_t)((1 << 14)/60.) >> 14);
        //Update_Tach(frequency);

        // Update fuel injector dead time
        fs_etpu_fuel_set_compensation_time(Fuel_Channels[0], Dead_Time);

        // Update MAP windows (where to sample MAP value)
        //uint32_t MAP_Angle;
// removed lookup for testing
#if 0
        MAP_Angle = 13500; // 540 degrees // Table_Lookup_JZ(RPM, Load, 0, MAP_Angle_Table); 

        for (i = 0; i < N_MAP_windows; ++i) {
            MAP_Angles[i*2] = MAP_Angle + Cyl_Angle_eTPU[i];
 
            // update eTPU 
            fs_etpu_knock_window_update ( 
               WINDOW0_CHANNEL,
               1,  // CAM channel
               i, 
               MAP_Angles[i*2], 
               MAP_Angles[i*2+1] );
        }
#endif

        task_wait(11);          // allow others tasks to run
    }                           // for      

    task_close();
}                               /* end of spark_task() */

/**********************************************************************************/
/*                 Fuel Task  - done in cycle fractions                           */
/**********************************************************************************/

#define CRANK_VOLTAGE 11
#define Run_Threshold 250      // RPM below this then not running

void Fuel_Task(void) {
    static uint16_t Update_Count;
    static uint16_t Update_Angle;
    static uint16_t Inv_Update_Count;
    static int16_t Corr;
    static uint24_t Pulse_Width;        // should be global
    static uint32_t error_code;

    task_open();               // standard OS entry - NOTE: no non-static local variables!

    for (;;) {

       //if the engine is not turning or the engine position is not known shut off the fuel channels
        if ((RPM == 0) || (fs_etpu_eng_pos_get_engine_position_status() != FS_ETPU_ENG_POS_FULL_SYNC))
        {
     
            int i;
            for (i = 0; i < N_Cyl; ++i)
                fs_etpu_fuel_switch_off(Fuel_Channels[i]);
            task_wait(10);      // delay for approx 10 msec and let other tasks run
        } else {

            // at low rpms, update calcs more often, in terms of degrees of engine rotation 
            if (RPM > 12000) {
                Update_Angle = 720;
                Update_Count = 1;
                Inv_Update_Count = (uint32_t) (1.0 * (1 << 12));
            } else if (RPM > 6000) {
                Update_Angle = 360;
                Update_Count = 2;
                Inv_Update_Count = (uint32_t) (0.5 * (1 << 12));
            } else if (RPM > 3000) {
                Update_Angle = 180;
                Update_Count = 4;
                Inv_Update_Count = (uint32_t) (0.25 * (1 << 12));
            } else if (RPM > 1500) {
                Update_Angle = 90;
                Update_Count = 8;
                Inv_Update_Count = (uint32_t) (0.125 * (1 << 12));
            } else if (RPM > 750) {
                Update_Angle = 45;
                Update_Count = 16;
                Inv_Update_Count = (uint32_t) (0.0625 * (1 << 12));
            } else if (RPM > 600) {
                Update_Angle = 36;
                Update_Count = 20;
                Inv_Update_Count = (uint32_t) (0.05 * (1 << 12));
            } else {
                Update_Angle = 18;
                Update_Count = 40;
                Inv_Update_Count = (uint32_t) (0.025 * (1 << 12));
            }

            static int i;
            for (i = 0; i < Update_Count; i++) {    // loop for 1 crank cycle

                // calc fuel pulse width
//remove for testing

                Pulse_Width = 500000;//Max_Inj_Time;	removed for testing	// base value in microseconds bin 0

                // apply various adjustments
                if ((V_Batt < (CRANK_VOLTAGE << 10)) && (RPM < Run_Threshold)) {        // Engine Cranking
//table removed for testing 
                    Corr = 6;// 10% bin 6// Table_Lookup_JZ(TPS, 0, Man_Crank_Corr_Table);       // get manual crank correction if cranking
                    Pulse_Width = (Pulse_Width * Corr) >> 6;			// table is bin 6
                }

                // engine temp correction
//table removed for testing 
                Corr = 8192; // 50% bin 14 //Table_Lookup_JZ(V_CLT, 0, CLT_Table);
                Pulse_Width = (Pulse_Width * Corr) >> 14; 	// correct bin point back to bin 0

                // RPM correction
//table removed for testing 
                Corr = 6553; // 40% bin 14// Table_Lookup_JZ(RPM, 0, Eng_Model_Table);
                Pulse_Width = (Pulse_Width * Corr) >> 14;

                // TODO - add load sense method selection and calcs. This only works right wiht 1 bar MAP 
                // Load = Get_Load();
//removed for testing 
                //Load = MAP[1];
                Pulse_Width = (Pulse_Width * Load) >> 14;

                // air temperature correction
                Pulse_Width = (Pulse_Width * IAT) >> 14;

                // JZ TODO - IMO, Inv_Update_Count should be based on delta angle clock, not a count
//removed for testing 
                //Corr = Accel_Enrich(Inv_Update_Count,Pulse_Width);  // adjust for enrichment/derichment based on rotation
                //Pulse_Width = (Pulse_Width + Corr);	// add (unlike other multiplies)

                // TODO adjust based on O2 sensor data
                // Corr = O2_Fuel();
                // Pulse_Width = (Pulse_Width * Corr) >> 14;

                // Assume fuel pressure is constant

                // Fuel pulse width calc is done

                // where should pulse end
//removed table for testing
                //Inj_End_Angle = 13500; //Table_Lookup_JZ(RPM, Load, Inj_End_Angle_Table);
                //Calculate angles for eTPU use, must reference the missing tooth not TDC
                //uint32_t Inj_End_Angle_eTPU = ( 13500+ Engine_Position);  // Inj_End_Angle
                //if (Inj_End_Angle_eTPU > 72000)
                 //  Inj_End_Angle_eTPU -= 72000;

                // Calculate an approprite re-calculation angle for the current injection Inj_End_Angle 
                //uint32_t Temp1 = ((RPM * Pulse_Width) >> 14);    //  * 1.2;       // 1.2 is to give the processor time to do the math
                //uint32_t Temp2 = (uint32_t) (.0006 * (1 << 12)); // conversion factor to get Temp1 into deg x 100
                //uint32_t Angle_Temp = 72000 - ((Temp1 * Temp2) >> 12);
                //uint32_t Fuel_Recalc_Angle_eTPU = (Inj_End_Angle_eTPU + Angle_Temp);
                //if (Fuel_Recalc_Angle_eTPU > 72000)
                //  Fuel_Recalc_Angle_eTPU -= 72000;

                //TODO - Cylinder Trim math and updates
                //TODO - Staged injection math and updates

Pulse_Width = 30000;//for testing
                // tell eTPU to use new values
                uint32_t j;
                for (j = 0; j < N_Injectors; ++j) {
                   error_code = 1; 
                   while (error_code != 0)// This tryies until the channel actual updated
                   {
                   error_code = fs_etpu_fuel_set_injection_time(Fuel_Channels[j], Pulse_Width);
                   }

                    fs_etpu_fuel_switch_on(Fuel_Channels[j]);     // Turn on fule channels
                }
                // These are global across channels, so only do it once
                //fs_etpu_fuel_set_normal_end_angle(Fuel_Channels[0], Inj_End_Angle_eTPU);  // degrees * 100
                //fs_etpu_fuel_set_recalc_offset_angle(Fuel_Channels[0], Fuel_Recalc_Angle_eTPU);   // degrees * 100

                task_wait_id(1, Update_Angle); // delay for approx n more degrees of crank angle and let other tasks run
            }                   // for
        }                       // if
    }                           // for

    task_close();               // OS terminator - never reached
}                               /* end of fuel_task() */

//#endif
#endif

